#include <joyos.h>
#include <math.h>
#include "define.h"
#include "roboturn.h"
#include "robomove.h"

/**
 * "shaftToAngle" converts the number of shafts detected to the degree of angle
 * turned by the robot
 *
 * @requires shaft >= 0
 * @effects return an angle in degrees predicted to be turned with the given shaft number
 * @modifies none
 */
int shaftToAngle(int16_t shaft){
	double completeTurnDist = ROBOT_WIDTH * M_PI;
	double wheelTurned = (double)shaft / (double)SHAFT_PER_WHEEL;
	double circumTurned = DIST_PER_WHEEL * wheelTurned;
	
	return (int)((circumTurned / completeTurnDist) * 360);
}

int angleToShaft(int16_t degree){
	double completeTurnDist = ROBOT_WIDTH * M_PI;
	double circumTurned = ((double)degree / 360) * completeTurnDist; // the circumference dist turned by robot
	double wheelTurned = circumTurned / DIST_PER_WHEEL;
	int sign = (degree > 0) ? 1 : -1;
	
	return (int)(sign * wheelTurned * SHAFT_PER_WHEEL);
}
/**
 * "turnWithShaft" turns the robot to the given degree.
 *
 * The turn will depend on a mix of error detections from gyro and shaft 
 * encoders from both wheels
 *
 * @requires gyro must have been calibrated
 * @effects the robot is turned to the given degree
 * @modifies none
 */
//Precise turning angle only right wheel will turn
void turnWithShaft(int16_t degree){
	bool turn_left;
	roboStop();
	encoder_reset(ENCODER_PORT_LEFT);
	encoder_reset(ENCODER_PORT_RIGHT);

	int16_t turn_vel_right;
	int16_t turn_vel_left;
	
	if (degree >= 0)
		turn_left = true;
	else{
		turn_left = false;
		degree = -degree;
	}
	int shaftToGet;
	if(degree <= 90){
		shaftToGet = angleToShaft(degree) + 30;
	}else{
		shaftToGet = angleToShaft(degree) + 45;
	}

	int16_t beforeShaftRight = encoder_read(ENCODER_PORT_RIGHT);
	int16_t beforeShaftLeft = encoder_read(ENCODER_PORT_LEFT);
	int16_t currentShaft = 0;
	int16_t currentShaftRight = 0;
	int16_t currentShaftLeft = 0;
			
	status = 2;	
	
	if (turn_left){
		turn_vel_right= MAX_TURN_MOTOR_VEL;
		while(currentShaft <  shaftToGet ){
			//if (check_stuck == 0) {
				motorSetVel(MOTOR_PORT_RIGHT, turn_vel_right);
				motorSetVel(MOTOR_PORT_LEFT, -turn_vel_right);
				currentShaftRight = encoder_read(ENCODER_PORT_RIGHT) - beforeShaftRight;
				currentShaft = currentShaftRight;
			
			//printf_P(PSTR("\nL angle: %d R_vel: %d %d"), shaftToAngle(currentShaft)
						//	, turn_vel_right, encoder_read(ENCODER_PORT_RIGHT));
			//turn_vel_right = MIN_TURN_MOTOR_VEL+(int16_t)(((shaftToGet - currentShaft) / shaftToGet) * (MAX_TURN_MOTOR_VEL-MIN_TURN_MOTOR_VEL));
			//pause(25);
			//}
			/*
			// check stuck
			else {
				printf("stuck when turn");
				beforeStuckShaft = encoder_read(ENCODER_PORT_RIGHT);
				
				motor_set_vel(MOTOR_PORT_RIGHT, -120);
				motor_set_vel(MOTOR_PORT_LEFT, -120);
				
				pause(50);
				
				motor_brake(MOTOR_PORT_LEFT);
				motor_brake(MOTOR_PORT_RIGHT);
				
				afterStuckShaft = encoder_read(ENCODER_PORT_RIGHT);
				beforeShaftRight += (afterStuckShaft - beforeStuckShaft);
				currentShaft = encoder_read(ENCODER_PORT_RIGHT);
				
			}
			*/
		}
	}
	else{
		turn_vel_left= MAX_TURN_MOTOR_VEL;
		while(currentShaft <  shaftToGet ){
			//if (check_stuck == 0) {
				motorSetVel(MOTOR_PORT_LEFT, turn_vel_left);
				motorSetVel(MOTOR_PORT_RIGHT, -turn_vel_left);
				currentShaftLeft = encoder_read(ENCODER_PORT_LEFT) - beforeShaftLeft;
				currentShaft = currentShaftLeft;
				//printf_P(PSTR("\nR angle: %d L_vel: %d %d"), shaftToAngle(currentShaft)
				//				, turn_vel_left, encoder_read(ENCODER_PORT_LEFT));
				//turn_vel_left = MIN_TURN_MOTOR_VEL+(int16_t)(((shaftToGet - currentShaft) / shaftToGet) * (MAX_TURN_MOTOR_VEL-MIN_TURN_MOTOR_VEL));
				//pause(25);
			//}
			/*
			else {
				printf("stuck when turn");
				beforeStuckShaft = encoder_read(ENCODER_PORT_LEFT);
				
				motor_set_vel(MOTOR_PORT_RIGHT, -120);
				motor_set_vel(MOTOR_PORT_LEFT, -120);
				
				pause(50);
				
				motor_brake(MOTOR_PORT_LEFT);
				motor_brake(MOTOR_PORT_RIGHT);
				
				afterStuckShaft = encoder_read(ENCODER_PORT_LEFT);
				beforeShaftLeft += (afterStuckShaft - beforeStuckShaft);
				currentShaft = encoder_read(ENCODER_PORT_LEFT);
			}*/
		}
	}
	roboStop();
}

/**
 * "turnEncoderErr" returns the error caused by difference in rates of turns by both wheels
 *
 * The error is calculated based on the difference of shafts detected by the left and the
 * right wheel of the robot
 *
 * @requires none
 * @effects returns the error value based on the shafts detected by both shaft encoders
 * @modifies none
 */
double turnEncoderErr(){
	int16_t leftShaftTurn = encoder_read(ENCODER_PORT_LEFT);
	int16_t rightShaftTurn = encoder_read(ENCODER_PORT_RIGHT);
	return leftShaftTurn - rightShaftTurn;
}

/**
 * "turnGyroErr" returns the error caused by difference in expected shafts and shafts actually
 * detected up to the point. The expected value is based on the angle measured by the gyro
 *
 * The error is measured on the encoder with the given port
 *
 * @requires gyro must be calibrated before hand
 * @effects returns the error value based on the shafts detected on the encoder of the given port
 * @modifies none
 */
double turnGyroErr(int16_t startAngle, uint8_t encoderPort){
	int16_t turnedAngle = getCurrentAngle() - startAngle;
	int16_t expectedShaft = angleToShaft(turnedAngle);
	
	int16_t turnedShaft = encoder_read(encoderPort);
	
	return expectedShaft - turnedShaft;
}

/*
 * "turn" turns the robot to the specified angle.
 * Angles are positive when turned counter-clockwise and negative when turned clockwise.
 * The angle is 0 when the direction is right in front of the robot.
 * 
 * @requires none
 * @effects the robot is turned to the specified angle.
 * @modifies none
 */
/*
void turn(int16_t degree){
	bool turn_left;
	int16_t turn_motor_vel = 0;
	
	roboStop();
	
	if (degree >= 0)
		turn_left = true;
	else
		turn_left = false;
	int16_t beforeAngle = getCurrentAngle();
	int16_t currentAngle = 0;
	int start_time = get_time();
	status = 2;
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);
	if (turn_left){
		while(currentAngle <= degree && (get_time()-start_time) < TURN_TIME_LIMIT){
			//printf_P(PSTR("\nangle: %d"), currentAngle);
			turn_motor_vel = (int)((fabs(degree - currentAngle) / degree) * MAX_TURN_MOTOR_VEL);
			motor_set_vel(MOTOR_PORT_RIGHT, turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, -turn_motor_vel);
			currentAngle = getCurrentAngle() - beforeAngle;
			pause(50);
		}
	}
	else{
		while(currentAngle >= degree && (get_time()-start_time) < TURN_TIME_LIMIT){
			//printf_P(PSTR("\nangle: %d R_velocity: %d"), currentAngle, turn_motor_vel);
			turn_motor_vel = (int16_t)((fabs(degree - currentAngle) / degree) * MAX_TURN_MOTOR_VEL);
			motor_set_vel(MOTOR_PORT_RIGHT, -turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, turn_motor_vel);
			currentAngle = getCurrentAngle() - beforeAngle;
			pause(50);
		}
	}
	roboStop();
}
*/

/*
 * "turnRight" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned clockwise
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */
void turnRight(int16_t degree){
	turnWithShaft(-degree);
}

/*
 * "turnLeft" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned counter-clockwise
 * 
 * It works exactly the same as "turn"
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */

void turnLeft(int16_t degree){
	turnWithShaft(degree);
}

/**
 * turnToOrient turns the robot to the given orientation
 *
 * the currentSide will be updated to the default value, meaning that it
 * is the expected value for normal situation. The currentSide of value 4
 * will be manually adjusted in accident-handler functions.
 * 
 * @requires the robot is not obstructed in turning
 * @effects turns the robot to the given orientation
 * @modifies currentOrient, currentSide
 */
void turnToOrient(int8_t orientNum){
	if (currentOrient == orientNum)
		{}
	else if ((currentOrient == orientNum - 1) || (currentOrient == orientNum + 3)){
		turnRight(90);
		currentSide = currentOrient;
	}
	else if ((currentOrient == orientNum - 2) || (currentOrient == orientNum + 2)){
		if(initPlace ==0){
			turnLeft(180);
		}
		else{
			turnRight(180);
		}
	}
	else if ((currentOrient == orientNum -3) || (currentOrient == orientNum + 1)){
		turnLeft(90);
		currentSide = currentOrient;
	}
	else {
		printf("\nOrient Error");
		pause(1000);
	}
	//currentSide = currentOrient;
	currentOrient = orientNum;
}

/*
 * "calibrateGyro" lets the robot rest for a brief period of time and reset its angle back to zero
 *
 * @requires none
 * @effects the angle measured by gyroscope is reset to zero
 * @modifies the angle stored in gyroscope
 */
void calibrateGyro(void){
	printf_P (PSTR("\nStabilizing..."));
	pause(1500);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_MS_PER_DEG, GYRO_CALI_TIME);
}

/**
 * "getCurrentAngle" returns the angle that the robot has turned during the turn
 * 
 * @requires none
 * @effects returns the angle that the robot has turned during the turn
 * @modifies none
 */
int16_t getCurrentAngle(void){
	return gyro_get_degrees();
}
